#define _USE_MATH_DEFINES
#include <cmath>
#include "AlgoEvaluatorModule9EndPointCompletion.h"

// Helper function to calculate distance between two lat/lon points in meters using Haversine formula.
// TODO: Move this to a common utility file to avoid duplication.
static double haversine_distance(double lat1, double lon1, double lat2, double lon2) {
    const double R = 6371000.0; // Earth radius in meters
    auto to_radians = [](double degree) {
        return degree * M_PI / 180.0;
    };

    double lat1_rad = to_radians(lat1);
    double lon1_rad = to_radians(lon1);
    double lat2_rad = to_radians(lat2);
    double lon2_rad = to_radians(lon2);

    double dlat = lat2_rad - lat1_rad;
    double dlon = lon2_rad - lon1_rad;

    double a = sin(dlat / 2.0) * sin(dlat / 2.0) +
               cos(lat1_rad) * cos(lat2_rad) *
               sin(dlon / 2.0) * sin(dlon / 2.0);
    double c = 2.0 * atan2(sqrt(a), sqrt(1.0 - a));

    return R * c;
}

class algo::AlgoEvaluatorModule9EndPointCompletionPrivate
{
public:
};

algo::AlgoEvaluatorModule9EndPointCompletion::AlgoEvaluatorModule9EndPointCompletion()
	: d(new AlgoEvaluatorModule9EndPointCompletionPrivate)
{
}

algo::AlgoEvaluatorModule9EndPointCompletion::~AlgoEvaluatorModule9EndPointCompletion()
{
	delete d;
}

std::vector<algobase::EvaluateEnvirDataType> algo::AlgoEvaluatorModule9EndPointCompletion::getNeedType()
{
	return std::vector<algobase::EvaluateEnvirDataType>();
}

int algo::AlgoEvaluatorModule9EndPointCompletion::exec(const algobase::Path& path, double& result, algobase::EvaluateModuleCoords& indexCoords)
{
	if (path.empty()) {
		// If the path is empty, it's infinitely far from the destination.
		result = 1.0e12; // A large penalty value.
		indexCoords.clear();
		return 0;
	}

	// Ensure the output coords vector has the correct size, even if unused.
	indexCoords.resize(path.size());
	for(size_t i = 0; i < path.size(); ++i) {
		indexCoords[i].Assign(path[i]);
	}

	if (path.empty()) {
		// If the path is empty, it's infinitely far from the destination.
		result = 1.0e12; // A large penalty value.
		return 0;
	}

	const auto& last_point = path.back();
	const auto& target_pos = getTaskConfig().mEndPos;

	// The raw value is the horizontal distance from the path's end to the target center.
	// Note: target_pos is [lon, lat], while PathCoord is [lon, lat, depth].
	result = calcScore(haversine_distance(last_point.mCoordZ[1], last_point.mCoordZ[0], target_pos[1], target_pos[0]));

	return 0;
}

double algo::AlgoEvaluatorModule9EndPointCompletion::calcScore(double rawValue) const
{
	double targetRadius_m = 1000.0; // Example radius
	return std::max(0.0, 1.0 - rawValue / targetRadius_m) * getWeight();
}

double algo::AlgoEvaluatorModule9EndPointCompletion::calcRaw(const std::vector<double>& rawValues) const
{
	return 0.0;
}
